#include "problem.h"
#include <iostream>

#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <vector>
#include <cmath>
using namespace Eigen;

std::tuple<Vector3d, Matrix3d> relative_orientation(const DataRelativeOrientation &data) {
    // 1. 给定初值
    Vector3d B(data.Bx, 0, 0);
    Vector3d r = Vector3d::Zero(); // 采用 rodrigues 转角
    Vector2d uv(0, 0);
    double f = data.f;

    // 2. 采用矩阵行列式作为观测值求解
    
    // 3. 返回解
    Matrix3d R = rodrigues(r);
    B = Vector3d(B.x(), B.x() * uv(0), B.x() * uv(1));

    return std::tuple<Vector3d, Matrix3d>(B, R);
}

std::tuple<Vector3d, Matrix3d> relative_orientation_golden(const DataRelativeOrientation &_) {
    DataIntersection data = generate_intersection();
    Vector3d B = data.R_eo[0].transpose() * (data.X_eo[1] - data.X_eo[0]);
    Matrix3d R = data.R_eo[0].transpose() * data.R_eo[1];

    return std::make_tuple(B, R);
}
